
#include "fc_error.h"
#include "zino.h"
#include "usercmd.h"
#include <rtthread.h>
#define DBG_TAG "fc_error"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

uint32_t rgb_state = 0;
uint16_t rgb_state_show = 0;
uint32_t fc_error_flag = 0;
// code uint8_t rainColor[21] = { 255, 0, 0, 255, 102, 0, 255, 255, 51, 51, 255, 51, 51, 51, 255, 102, 0, 204, 153, 51, 153 };
const uint8_t blue_color[3] = {0, 0, 255};     // �������� ������ģ�������������Ƶ
const uint8_t greenColor[3] = {0, 255, 0};     // �����̵� �����������У������������ ������������
const uint8_t yellow_color[3] = {255, 255, 0}; // �Ƶ� ������ң���У����������ź�
const uint8_t red_color[3] = {255, 0, 0};      // ��Դ�� ����У׼�У� �����͵���
const uint8_t purple_color[3] = {102, 0, 204}; // ������������Ӳ�������⣬�����ڴ�ӡ
// NOTE - �����ã���ͻ��޹�
const uint8_t white_color[3] = {255, 255, 255}; // ��������

struct rgbColorState
{
    const uint8_t *color;
    uint8_t mode;
};
enum
{
    RGB_STATE_MODE_FAST = 0,
    RGB_STATE_MODE_SLOW,
    RGB_STATE_MODE_BREATH,
};

/**
 * @brief
 *      FC_ACC_GYRO_HARDFAULT = 0, // 陀螺仪与加速度计错误
        FC_BARO_HARDFAULT,         // 气压计错误
        FC_RC_HARDFAULT,           // 无线模块错误
        FC_OPTICALFLOW_HARDFAULT,  // 光流模块错误
        FC_COMPASS_HARDFAULT,      // 罗盘模块错误
        FC_IMU_CALIBRATING,        // 水平校准带中
        FC_RC_PAIRING,             // 无线对频中
        FC_RC_LOST,                // 无线无信号
        FC_COMPASS_CALIBRATING,    // 罗盘校准中
        FC_STANDBY,                // 一切正常，待机...(新手室内)
        FC_OK_TO_ARM,              // 解锁中
        FC_EMERGENCY_STOP,         // 急停
        FC_ARMED,                  // 解锁了
        FC_ARMING_ALERK,           // 解锁警报
        FC_MODE_STANDARD_INDOOR,   // 标准室内
        FC_MODE_STANDARD_OUTDOOR,  // 标准室外
        FC_LOW_POWER,              // 低电量
        FC_LASER_HARDFAULT,
        FC_PRO_STANDBY, // 编程一切正常，待机...(新手室内)
        FC_FACTORY_TEST,//出厂测试
        FC_ERROR_COUNT,
 *
 */
struct rgbColorState rgbColorState[FC_ERROR_COUNT] =
    {

        {white_color, RGB_STATE_MODE_FAST},    // FC_ACC_GYRO_HARDFAULT
        {white_color, RGB_STATE_MODE_FAST},    // FC_BARO_HARDFAULT,
        {blue_color, RGB_STATE_MODE_FAST},     // FC_RC_HARDFAULT
        {white_color, RGB_STATE_MODE_FAST},    // FC_OPTICALFLOW_HARDFAULT
        {white_color, RGB_STATE_MODE_FAST},    // FC_COMPASS_HARDFAULT
        {red_color, RGB_STATE_MODE_FAST},      // FC_IMU_CALIBRATING,
        {blue_color, RGB_STATE_MODE_SLOW},     // FC_RC_PAIRING
        {yellow_color, RGB_STATE_MODE_SLOW},   // FC_RC_LOST
        {red_color, RGB_STATE_MODE_FAST},      // FC_COMPASS_CALIBRATING
        {blue_color, RGB_STATE_MODE_BREATH},   // FC_STANDBY
        {greenColor, RGB_STATE_MODE_FAST},     // FC_OK_TO_ARM
        {yellow_color, RGB_STATE_MODE_FAST},   // FC_EMERGENCY_STOP
        {greenColor, RGB_STATE_MODE_BREATH},   // FC_ARMED
        {greenColor, RGB_STATE_MODE_FAST},     // FC_ARMING_ALERK
        {blue_color, RGB_STATE_MODE_BREATH},   // FC_MODE_STANDARD_INDOOR
        {blue_color, RGB_STATE_MODE_BREATH},   // FC_MODE_STANDARD_OUTDOOR
        {red_color, RGB_STATE_MODE_SLOW},      // FC_LOW_POWER
        {white_color, RGB_STATE_MODE_FAST},    // FC_LASER_HARDFAULT
        {purple_color, RGB_STATE_MODE_BREATH}, // FC_PRO_STANDBY
        {white_color, RGB_STATE_MODE_SLOW},    // FC_FACTORY_TEST
                                               // FC_ERROR_COUNT,
};

void rgb_set_color_sum(uint8_t sum, uint8_t r, uint8_t g, uint8_t b)
{

    // uint8_t test=testoptQual+1;
    // sum = testoptQual;

    if (sum >= 5)
    {
        sum = 5;
    }

    for (int i = 1; i <= 5; i++)
    {
        rgb_set_color(i - 1, 0, 0, 0);
    }

    for (int i = 1; i <= sum; i++)
    {

        rgb_set_color(i - 1, r, g, b);
    }
}

void rgb_state_handle()
{
#define SHOW_TIME 32
    static uint8_t stateCnt;
    static uint8_t rgb_scnt = 0;

    rgb_state = fc_error_flag;

    if ((fc_error_flag & ~((uint32_t)(1 << FC_STANDBY))))//NO ERROR
    {
        FC_ERROR_RESET(FC_STANDBY);
    }
    else
    {
        FC_ERROR_SET(FC_STANDBY);
    }


    if (rgb_scnt++ >= SHOW_TIME)
    {
        rgb_scnt = 0;
        stateCnt++;
    }
    if ((1 << stateCnt) & rgb_state)
    {
        uint8_t r = 0, g = 0, b = 0;
        // LOG_D("rgb_state:%s %d\n", rgb_state_str[stateCnt], (int)stateCnt);
        switch (rgbColorState[stateCnt].mode)
        {
        case RGB_STATE_MODE_FAST:
            // LOG_D("RGB_STATE_MODE_FAST %d\n", (int)rgb_scnt);
            if ((rgb_scnt % (SHOW_TIME / 4)) < (SHOW_TIME / 8))
            {
                // LOG_D("fast %d\n", (int)rgb_scnt);
                r = rgbColorState[stateCnt].color[0];
                g = rgbColorState[stateCnt].color[1];
                b = rgbColorState[stateCnt].color[2];
            }
            break;
        case RGB_STATE_MODE_SLOW:
            // LOG_D("RGB_STATE_MODE_SLOW %d %d\n", (int)rgb_scnt, (int)stateCnt);
            if (rgb_scnt < (SHOW_TIME / 2))
            {
                r = rgbColorState[stateCnt].color[0];
                g = rgbColorState[stateCnt].color[1];
                b = rgbColorState[stateCnt].color[2];
            }
            break;
        case RGB_STATE_MODE_BREATH:
            // LOG_D("RGB_STATE_MODE_BREATH %d\n", (int)rgb_scnt);
            if (rgb_scnt < (SHOW_TIME / 2))
            {
                r = rgbColorState[stateCnt].color[0] * rgb_scnt / (SHOW_TIME / 2);
                g = rgbColorState[stateCnt].color[1] * rgb_scnt / (SHOW_TIME / 2);
                b = rgbColorState[stateCnt].color[2] * rgb_scnt / (SHOW_TIME / 2);
            }
            else
            {
                r = rgbColorState[stateCnt].color[0] * (SHOW_TIME - rgb_scnt) / (SHOW_TIME / 2);
                g = rgbColorState[stateCnt].color[1] * (SHOW_TIME - rgb_scnt) / (SHOW_TIME / 2);
                b = rgbColorState[stateCnt].color[2] * (SHOW_TIME - rgb_scnt) / (SHOW_TIME / 2);
            }
            // LOG_D("Rgb:%d,%d,%d \n", (int)r, (int)g, (int)b);
            break;
        default:
            break;
        }
        rgb_set_color_sum(5, r, g, b);
    }
    else
    {
        while (!((1 << stateCnt) & rgb_state))
        {
            stateCnt++;
            if (stateCnt >= FC_ERROR_COUNT)
            {
                stateCnt = 0;
                break;
            }
        }
    }
    rgb_show();
}
void fc_error_info_mesg(void)
{

    if (FC_ERROR_CHECK(FC_ACC_GYRO_HARDFAULT))
        LOG_E("\n*%d* FC_ACC_GYRO_HARDFAULT\n", FC_ACC_GYRO_HARDFAULT);
    if (FC_ERROR_CHECK(FC_BARO_HARDFAULT))
        LOG_E("\n*%d* FC_BARO_HARDFAULT\n", FC_BARO_HARDFAULT);
    if (FC_ERROR_CHECK(FC_RC_HARDFAULT))
        LOG_E("\n*%d* FC_RC_HARDFAULT\n", FC_RC_HARDFAULT);
    if (FC_ERROR_CHECK(FC_OPTICALFLOW_HARDFAULT))
        LOG_E("\n*%d* FC_OPTICALFLOW_HARDFAULT\n", FC_OPTICALFLOW_HARDFAULT);
    if (FC_ERROR_CHECK(FC_COMPASS_HARDFAULT))
        LOG_E("\n*%d* FC_COMPASS_HARDFAULT\n", FC_COMPASS_HARDFAULT);
    if (FC_ERROR_CHECK(FC_IMU_CALIBRATING))
        LOG_E("\n*%d* FC_IMU_CALIBRATING\n", FC_IMU_CALIBRATING);
    if (FC_ERROR_CHECK(FC_RC_PAIRING))
        LOG_E("\n*%d* FC_RC_PAIRING\n", FC_RC_PAIRING);
    if (FC_ERROR_CHECK(FC_RC_LOST))
        LOG_E("\n*%d* FC_RC_LOST\n", FC_RC_LOST);
    if (FC_ERROR_CHECK(FC_IMU_CALIBRATING))
        LOG_E("\n*%d* FC_IMU_CALIBRATING\n", FC_IMU_CALIBRATING);
    if (FC_ERROR_CHECK(FC_COMPASS_CALIBRATING))
        LOG_E("\n*%d* FC_COMPASS_CALIBRATING\n", FC_COMPASS_CALIBRATING);
    if (FC_ERROR_CHECK(FC_LASER_HARDFAULT))
        LOG_E("\n*%d* FC_LASER_HARDFAULT\n", FC_LASER_HARDFAULT);
    if (FC_ERROR_CHECK(FC_STANDBY))
        LOG_E("\n*%d* FC_STANDBY\n", FC_STANDBY);
    if (FC_ERROR_CHECK(FC_PRO_STANDBY))
        LOG_E("\n*%d* FC_PRO_STANDBY\n", FC_PRO_STANDBY);
    // if (isFC_NO_ERROR())
    // {
    //     if (fc_mode_record == 0)
    //         usartPrintf("\n*%d* FC_STANDBY\n", fc_mode_record);
    //     else if (fc_mode_record == 1)
    //         usartPrintf("\n*%d* FC_MODE_STANDARD_INDOOR\n", fc_mode_record);
    //     else if (fc_mode_record == 2)
    //         usartPrintf("\n*%d* FC_MODE_STANDARD_OUTDOOR\n", fc_mode_record);

    if (FC_ERROR_CHECK(FC_OK_TO_ARM))
        LOG_E("\n*%d* FC_OK_TO_ARM\n", FC_OK_TO_ARM);
    if (FC_ERROR_CHECK(FC_EMERGENCY_STOP))
        LOG_E("\n*%d* FC_EMERGENCY_STOP\n", FC_EMERGENCY_STOP);
    if (FC_ERROR_CHECK(FC_ARMED))
        LOG_E("\n*%d* FC_ARMED\n", FC_ARMED);
    if (FC_ERROR_CHECK(FC_ARMING_ALERK))
        LOG_E("\n*%d* FC_ARMING_ALERK\n", FC_ARMING_ALERK);
    //     if (FC_ERROR_CHECK(FC_LOW_POWER))
    //         LOG_E("\n*%d* FC_LOW_POWER\n", FC_LOW_POWER);
    //     usartPrintf("\nRC_LOST_CHECKED:%d\n", RC_LOST_CHECKED);
    // }
    // LOG_D("fc_error_flag = %08x", fc_error_flag);
    // LOG_E("rssi %d\n",usercmd.rc_rssi);
}

void fc_error_handle_task(void *parameter)
{
    while (1)
    {
        static uint8_t cnt = 0;
        if (cnt++ > 20)
        {
            cnt = 0;
            // fc_error_info_mesg();
        }
        rgb_state_handle();
        rt_thread_mdelay(50);
    }
}
#if 0
int fc_error_handle_task_entry(void)
{

    rt_thread_t fc_error_handle_thread = rt_thread_create("fc_error_handle", fc_error_handle_task, RT_NULL, 512, 10, 10);
    if (fc_error_handle_thread != RT_NULL)
    {
        rt_thread_startup(fc_error_handle_thread);
    }
    else
    {
        LOG_E("fc_error_handle_thread create failed.");
    }
    return 0;
}
ZINO_APP_EXPORT(fc_error_handle_task_entry);
#endif
